#include "stm32f1xx.h"
#include "mpu6050_test.h"
#include "mpu6050.h"
#include "USART.h"
#include "stdio.h"
#include "oled.h"
#include "oledfont.h"
//#include "task.h"

/*串口重定向*/
int fputc(int ch,FILE *f)
{
    HAL_UART_Transmit(&huart1,(uint8_t*)&ch,1,0xFFFF);
    return ch;
}

static void MPU6050_Proc(void);

/*测试欧拉角（VOFA）*/
void MPU6050_EularAngleTest(void)
{
    MPU6050_Init();
	while(1)
    {
        MPU6050_Proc();
    }
	
}

/*每间隔10ms把欧拉角计算结果发送给上位机*/
static void MPU6050_Proc(void)
{
    //PERIODIC_TASK(10);
//    static uint32_t nxt = 0;//下次程序调用时间
//    if (HAL_GetTick() < nxt) return;

    float ax = MPU6050_GetAx();
    float ay = MPU6050_GetAy();
    float az = MPU6050_GetAz();
    float gx = MPU6050_GetGx();
    float gy = MPU6050_GetGy();
    float gz = MPU6050_GetGz();
    float temperature = MPU6050_GetTemperature();
    float yaw = MPU6050_GetYaw();
    float pitch = MPU6050_GetPitch();
    float roll = MPU6050_GetRoll();
	
	/*串口打印*/
//    printf("%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", ax, ay, az, gx, gy, gz, temperature, yaw, pitch, roll);
	OLED_Init();
	OLED_Clear();
	/*OLED显示*/
	char buffer[8];
    sprintf(buffer, "yaw: %.2f", yaw);	
    OLED_ShowString(0, 0, buffer, 16, 0);
    sprintf(buffer, "pitch: %.2f", pitch);	
    OLED_ShowString(0, 2, buffer, 16, 0);
    sprintf(buffer, "roll: %.2f", roll);	
    OLED_ShowString(0, 4, buffer, 16, 0);	
	
//    nxt += 10; //设置下次调用时间
//    if (nxt > 1000) nxt = 0; //防止溢出
}
